#include "Collision.h"

bool Collision::hasCollided(RigidBody &b){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> up = center + vector3<float>(0,sizeY,0),
                   down = center + vector3<float>(0,-sizeY,0),
                   left = center + vector3<float>(-sizeX,0,0),
                   right =  center + vector3<float>(sizeX,0,0),
                   front = center + vector3<float>(0,0,sizeZ),
                   back = center + vector3<float>(0,0,-sizeZ),
                   leftdownback = center + vector3<float>(-sizeX,-sizeY,-sizeZ),
                   rightdownback = center + vector3<float>(sizeX,-sizeY,-sizeZ),
                   leftupback = center + vector3<float>(-sizeX,sizeY,-sizeZ),
                   rightupback = center + vector3<float>(sizeX,sizeY,-sizeZ),
                   leftdownfront = center + vector3<float>(-sizeX,-sizeY,sizeZ),
                   rightdownfront = center + vector3<float>(sizeX,-sizeY,sizeZ),
                   leftupfront = center + vector3<float>(-sizeX,sizeY,sizeZ),
                   rightupfront = center + vector3<float>(sizeX,sizeY,sizeZ);

    if(checkForVoxelCollision(b,up))return true;
    if(checkForVoxelCollision(b,down))return true;
    if(checkForVoxelCollision(b,left))return true;
    if(checkForVoxelCollision(b,right))return true;
    if(checkForVoxelCollision(b,back))return true;
    if(checkForVoxelCollision(b,front))return true;
    if(checkForVoxelCollision(b,leftdownback))return true;
    if(checkForVoxelCollision(b,rightdownback))return true;
    if(checkForVoxelCollision(b,leftupback))return true;
    if(checkForVoxelCollision(b,rightupback))return true;
    if(checkForVoxelCollision(b,leftdownfront))return true;
    if(checkForVoxelCollision(b,rightdownfront))return true;
    if(checkForVoxelCollision(b,leftupfront))return true;
    if(checkForVoxelCollision(b,rightupfront))return true;
    return false;

}

bool Collision::hasCollided(RigidBody &b, vector3<float>& normal){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> up = center + vector3<float>(0,sizeY,0),
                   down = center + vector3<float>(0,-sizeY,0),
                   left = center + vector3<float>(-sizeX,0,0),
                   right =  center + vector3<float>(sizeX,0,0),
                   front = center + vector3<float>(0,0,sizeZ),
                   back = center + vector3<float>(0,0,-sizeZ),
                   leftdownback = center + vector3<float>(-sizeX,-sizeY,-sizeZ),
                   rightdownback = center + vector3<float>(sizeX,-sizeY,-sizeZ),
                   leftupback = center + vector3<float>(-sizeX,sizeY,-sizeZ),
                   rightupback = center + vector3<float>(sizeX,sizeY,-sizeZ),
                   leftdownfront = center + vector3<float>(-sizeX,-sizeY,sizeZ),
                   rightdownfront = center + vector3<float>(sizeX,-sizeY,sizeZ),
                   leftupfront = center + vector3<float>(-sizeX,sizeY,sizeZ),
                   rightupfront = center + vector3<float>(sizeX,sizeY,sizeZ);

        bool contacted=false;
        normal=vector3<float>(0,0,0);

        if(Collision::checkForVoxelCollision(b,leftdownback))normal+=vector3<float>(SQRT_THIRD,SQRT_THIRD,SQRT_THIRD),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightdownback))normal+=vector3<float>(-SQRT_THIRD,SQRT_THIRD,SQRT_THIRD),contacted=true;
        if(Collision::checkForVoxelCollision(b,leftdownfront))normal+=vector3<float>(SQRT_THIRD,SQRT_THIRD,-SQRT_THIRD),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightdownfront))normal+=vector3<float>(-SQRT_THIRD,SQRT_THIRD,-SQRT_THIRD),contacted=true;
        if(Collision::checkForVoxelCollision(b,leftupback))normal+=vector3<float>(SQRT_THIRD,-SQRT_THIRD,SQRT_THIRD),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightupback))normal+=vector3<float>(-SQRT_THIRD,-SQRT_THIRD,SQRT_THIRD),contacted=true;
        if(Collision::checkForVoxelCollision(b,leftupfront))normal+=vector3<float>(SQRT_THIRD,-SQRT_THIRD,-SQRT_THIRD),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightupfront))normal+=vector3<float>(-SQRT_THIRD,-SQRT_THIRD,-SQRT_THIRD),contacted=true;

        //axis vectors more significant, else compose normal out of corner cases
        if(Collision::checkForVoxelCollision(b,down))normal=vector3<float>(0,1,0),contacted=true;
        if(Collision::checkForVoxelCollision(b,up))normal=vector3<float>(0,-1,0),contacted=true;
        if(Collision::checkForVoxelCollision(b,left))normal=vector3<float>(1,0,0),contacted=true;
        if(Collision::checkForVoxelCollision(b,right))normal=vector3<float>(-1,0,0),contacted=true;
        if(Collision::checkForVoxelCollision(b,back))normal=vector3<float>(0,0,1),contacted=true;
        if(Collision::checkForVoxelCollision(b,front))normal=vector3<float>(0,0,-1),contacted=true;

        normal = normal.unit();

        return contacted;
}




bool Collision::hasCollidedUp(RigidBody& b){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> up = center + vector3<float>(0,sizeY,0),
                   leftupback = center + vector3<float>(-sizeX,sizeY,-sizeZ),
                   rightupback = center + vector3<float>(sizeX,sizeY,-sizeZ),
                   leftupfront = center + vector3<float>(-sizeX,sizeY,sizeZ),
                   rightupfront = center + vector3<float>(sizeX,sizeY,sizeZ);

    return (checkForVoxelCollision(b,up) ||
            checkForVoxelCollision(b,leftupback)  ||
            checkForVoxelCollision(b,rightupback)  ||
            checkForVoxelCollision(b,leftupfront)  ||
            checkForVoxelCollision(b,rightupfront));

}




bool Collision::hasCollidedDown(RigidBody& b){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> down = center + vector3<float>(0,-sizeY,0),
                   leftdownback = center + vector3<float>(-sizeX,-sizeY,-sizeZ),
                   rightdownback = center + vector3<float>(sizeX,-sizeY,-sizeZ),
                   leftdownfront = center + vector3<float>(-sizeX,-sizeY,sizeZ),
                   rightdownfront = center + vector3<float>(sizeX,-sizeY,sizeZ);

    return (checkForVoxelCollision(b,down) ||
            checkForVoxelCollision(b,leftdownback)  ||
            checkForVoxelCollision(b,rightdownback)  ||
            checkForVoxelCollision(b,leftdownfront)  ||
            checkForVoxelCollision(b,rightdownfront));
}




bool Collision::hasCollidedLeft(RigidBody& b){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> left = center + vector3<float>(-sizeX,0,0),
                   leftdownback = center + vector3<float>(-sizeX,-sizeY,-sizeZ),
                   leftupback = center + vector3<float>(-sizeX,sizeY,-sizeZ),
                   leftdownfront = center + vector3<float>(-sizeX,-sizeY,sizeZ),
                   leftupfront = center + vector3<float>(-sizeX,sizeY,sizeZ);

    return (checkForVoxelCollision(b,left) ||
            checkForVoxelCollision(b,leftupback)  ||
            checkForVoxelCollision(b,leftdownback)  ||
            checkForVoxelCollision(b,leftupfront)  ||
            checkForVoxelCollision(b,leftdownfront));
}




bool Collision::hasCollidedRight(RigidBody& b){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> right =  center + vector3<float>(sizeX,0,0),
                   rightdownback = center + vector3<float>(sizeX,-sizeY,-sizeZ),
                   rightupback = center + vector3<float>(sizeX,sizeY,-sizeZ),
                   rightdownfront = center + vector3<float>(sizeX,-sizeY,sizeZ),
                   rightupfront = center + vector3<float>(sizeX,sizeY,sizeZ);

    return (checkForVoxelCollision(b,right) ||
            checkForVoxelCollision(b,rightupfront)  ||
            checkForVoxelCollision(b,rightdownfront)  ||
            checkForVoxelCollision(b,rightupback)  ||
            checkForVoxelCollision(b,rightdownback));
}




bool Collision::hasCollidedBack(RigidBody& b){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> back = center + vector3<float>(0,0,-sizeZ),
                   leftdownback = center + vector3<float>(-sizeX,-sizeY,-sizeZ),
                   rightdownback = center + vector3<float>(sizeX,-sizeY,-sizeZ),
                   leftupback = center + vector3<float>(-sizeX,sizeY,-sizeZ),
                   rightupback = center + vector3<float>(sizeX,sizeY,-sizeZ);

    return (checkForVoxelCollision(b,back) ||
            checkForVoxelCollision(b,leftupback)  ||
            checkForVoxelCollision(b,rightupback)  ||
            checkForVoxelCollision(b,leftdownback)  ||
            checkForVoxelCollision(b,rightdownback));
}




bool Collision::hasCollidedFront(RigidBody& b){
    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +COLLIDE_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +COLLIDE_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +COLLIDE_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> front = center + vector3<float>(0,0,sizeZ),
                   leftdownfront = center + vector3<float>(-sizeX,-sizeY,sizeZ),
                   rightdownfront = center + vector3<float>(sizeX,-sizeY,sizeZ),
                   leftupfront = center + vector3<float>(-sizeX,sizeY,sizeZ),
                   rightupfront = center + vector3<float>(sizeX,sizeY,sizeZ);

    return (checkForVoxelCollision(b,front) ||
            checkForVoxelCollision(b,leftupfront)  ||
            checkForVoxelCollision(b,rightupfront)  ||
            checkForVoxelCollision(b,leftdownfront)  ||
            checkForVoxelCollision(b,rightdownfront));
}
